#include "stepdrv.h"

StepObiectDef YSO={0,0,0,0,0,0,0,0,0};
StepObiectDef XSO={0,0,0,0,0,0,0,0,0};
PIDDef XPID={0,0,0,0,0,0,0,0,0,0};
PIDDef YPID={0,0,0,0,0,0,0,0,0,0};

void YSO_Start(void)
{
	if(YSO.currentPulsePos == YSO.taragetPulsePos || YSO.stepStatus == BUSY)
	{
		return;
	}
	YSO.stepStatus = BUSY;
	if(YSO.currentPulsePos > YSO.taragetPulsePos)
	{
		//上
		YSO.nowDir=CCW;
	}
	else
	{
		//下
		YSO.nowDir=CW;
	}
	 HAL_GPIO_WritePin(TOP_DIR_GPIO_Port, TOP_DIR_Pin,YSO.nowDir);
	 //开启PWM中断
	 HAL_TIM_PWM_Start_IT(&htim3,TIM_CHANNEL_1);
	 //开启PWM
	 HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_1);
}

void XSO_Start(void)
{
//	if(XSO.currentPulsePos == XSO.taragetPulsePos || XSO.stepStatus == BUSY)
//	{
//		return;
//	}

	XSO.stepStatus = BUSY;        //电机忙碌状态
	if(XSO.currentPulsePos<XSO.taragetPulsePos)
	{
		//右
		XSO.nowDir=CW;
	}
	else
	{
		//左
		XSO.nowDir=CCW;
	}
	 HAL_GPIO_WritePin(BTM_DIR_GPIO_Port, BTM_DIR_Pin,XSO.nowDir);
	 //开启PWM中断
	 HAL_TIM_PWM_Start_IT(&htim4,TIM_CHANNEL_1);
	 //开启PWM
	 HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
}

void XSO_End(void)
{
//	if(XSO.currentPulsePos <XSO.taragetPulsePos)
//	{
//		XSO.currentPulsePos ++;
//	}
//	else
//	{
//		XSO.currentPulsePos --;
//	}
	XSO.stepStatus = FREE;     //电机空闲状态
	HAL_TIM_PWM_Stop(&htim4,TIM_CHANNEL_1);
	HAL_TIM_PWM_Stop_IT(&htim4,TIM_CHANNEL_1);
}

void YSO_End(void)
{
//	if(YSO.currentPulsePos <YSO.taragetPulsePos)
//	{
//		YSO.currentPulsePos ++;
//	}
//	else
//	{
//		YSO.currentPulsePos --;
//	}
	YSO.stepStatus = FREE;     //电机空闲状态
	HAL_TIM_PWM_Stop(&htim3,TIM_CHANNEL_1);
	HAL_TIM_PWM_Stop_IT(&htim3,TIM_CHANNEL_1);
}

void locatesmooth(void)
{
	if(CO.workStatus == BUSY)
	{
		return;
	}

	if(CO.pointNum>1)//多点路线
	{
		if(CO.loop<0)//一直转
		{
			CO.pointindex = (++CO.pointindex)%CO.pointNum;
		}
		else if(CO.loop==0)
		{
			CO.pointindex = 0;
		}
		else
		{
			CO.pointindex = (++CO.pointindex)%CO.pointNum;
		}
		if(CO.pointindex==(CO.pointNum-1)&&CO.loop>0)
		{
			CO.loop--;
		}
	}
	else //单点路线
	{
		CO.pointindex = 0 ;
	}
	XSO.taragetPulsePos = (CO.targetPointer+CO.pointindex)->xCoordinate;
	YSO.taragetPulsePos = (CO.targetPointer+CO.pointindex)->yCoordinate;

	XSO.distanceLength = XSO.taragetPulsePos-XSO.currentPulsePos;
	YSO.distanceLength = YSO.taragetPulsePos-YSO.currentPulsePos;
	if(XSO.distanceLength==0 && YSO.distanceLength==0)
	{
		CO.workStatus = FREE;
		return;
	}
	CO.workStatus = BUSY;
	if(XSO.distanceLength==0 || YSO.distanceLength==0)//横平竖直，不存在同步运动
	{
		XSO.stepDelayReload = 0;
		YSO.stepDelayReload = 0;
	}
	else if(absint(XSO.distanceLength) > absint(YSO.distanceLength))//靠近x移动
	{
		XSO.stepDelayReload = 0;
		YSO.stepDelayReload = (absfloat(XSO.distanceLength)/absfloat(YSO.distanceLength));
	}
	else if(absint(XSO.distanceLength) < absint(YSO.distanceLength))//靠近y移动
	{
		XSO.stepDelayReload = (absfloat(YSO.distanceLength)/absfloat(XSO.distanceLength));
		YSO.stepDelayReload = 0;
	}
	else
	{
		XSO.stepDelayReload = 0;
		YSO.stepDelayReload = 0;
	}
}

//PID参数设置函数
void PIDSetParam(PIDDef *hPID,float Kp,float Ki,float Kd,float Imax,int Deadzone,int Izone)
{
    hPID->Kp=Kp;
    hPID->Ki=Ki;
    hPID->Kd=Kd;
    hPID->IntegralMAX=Imax;
    hPID->Dead_zone=Deadzone;
    hPID->Integral_zone=Izone;
}

int Incremental_PID(int reality,int target,PIDDef* hPID)
{
    //计算本次误差
    hPID->Bias = target-reality;
    //更新上上次误差(上次误差传递给上上次误差）
    hPID->Prev_bias=hPID->Last_bias;
    //更新上次误差(本次误差传递给上次误差）
    hPID->Last_bias=hPID->Bias;
    //PID输出运算
    hPID->Pid_out =(hPID->Kp*(hPID->Bias-hPID->Last_bias))                  /* 比例环节 */
                  +(hPID->Ki*hPID->Bias)                                    /* 积分环节 */
                  +(-hPID->Kd*hPID->Bias-2*hPID->Last_bias+hPID->Prev_bias); /* 微分环节 */
    //返回整数结果
    return (int)hPID->Pid_out;
}


